#include "mult_lidar/neutron_detector.hpp"
#include <algorithm>
#include <cmath>

namespace mult_lidar {

    void NeutronDetector::setParameters(rclcpp::Node* node) {
        node->get_parameter("neutron.search_start_z", params_.search_start_z);
        node->get_parameter("neutron.search_end_z", params_.search_end_z);
        node->get_parameter("neutron.search_step", params_.search_step);
        node->get_parameter("neutron.height_range", params_.height_range);
        node->get_parameter("neutron.height_offset", params_.height_offset);
        node->get_parameter("neutron.min_points_in_slice", params_.min_points_in_slice);

        node->get_parameter("neutron.rect_min_area", params_.rect_min_area);
        node->get_parameter("neutron.rect_max_area", params_.rect_max_area);

        /* node->get_parameter("neutron.circle_min_radius", params_.circle_min_radius);
        node->get_parameter("neutron.circle_max_radius", params_.circle_max_radius);
        node->get_parameter("neutron.circle_min_points", params_.circle_min_points);

        node->get_parameter("neutron.center_merge_threshold", params_.center_merge_threshold); */
    }

    bool NeutronDetector::detectCenter(PointCloud cloud,
                                    geometry_msgs::msg::PointStamped& center_point) {
                
        if(cloud->empty()) {
            RCLCPP_WARN(rclcpp::get_logger("neutron_detector"), "Empty cloud for neutron detection!");
            return false;
        }

        //1.检测顶部区域
        float detected_z = NAN;
        auto neutron_cloud = detectTopRegion(cloud, detected_z);
        if(std::isnan(detected_z)) {
            RCLCPP_WARN(rclcpp::get_logger("neutron_detector"), "No top neutron cloud found!");
            return false;
        }
        if(static_cast<int>(neutron_cloud->size()) < params_.min_points_in_slice) {
            RCLCPP_WARN(rclcpp::get_logger("neutron_detector"), 
                                          "Not enough points out of %ld!", neutron_cloud->size());
            return false;
        }

        //2.投影到XY平面
        auto points_2d = projectTo2D(neutron_cloud);

        //3.拟合外框矩形
        cv::Point2f rect_center;
        if(!fitRectangleAndGetCenter(points_2d, rect_center)) {
            RCLCPP_WARN(rclcpp::get_logger("neutron_detector"), 
                                          "Failed to fit outer rectangle");
            return false;
        }

        /* //4.检测并拟合内部圆形
        auto inner_points = detectInnerCircleRegion(points_2d, outer_rect);
        cv::Point2f circle_center;
        float circle_radius;
        if(!fitCircle(inner_points, circle_center, circle_radius)) {
             RCLCPP_WARN(rclcpp::get_logger("neutron_detector"), 
                                          "Failed to fit inner circle");
            return false;
        }

        //5. 计算最终中心
        cv::Point2f rect_center(outer_rect.center.x, outer_rect.center.y);
        cv::Point2f final_center;
        computeFinalCenter(rect_center, circle_center, final_center);
 */
        //4.设置结果
        center_point.point.x = rect_center.x;
        center_point.point.y = rect_center.y;
        center_point.point.z = detected_z;
        center_point.header.frame_id = "map";

        RCLCPP_INFO(rclcpp::get_logger("neutron_detector"),
                    "Neutron Detector center: %.3f, %.3f, %.3f",
                    rect_center.x, rect_center.y, detected_z);
        return true;
    }


    NeutronDetector::PointCloud NeutronDetector::detectTopRegion(const PointCloud& cloud,
                                                                float& detected_z) {
        auto neutron_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        detected_z = NAN;

        //从上往下开始搜索
        for(float current_z = params_.search_start_z; 
            current_z > params_.search_end_z;
            current_z -= params_.search_step) {
            neutron_cloud->clear();

            //提取当前高度切片
            for(const auto& point : *cloud) {
                if(point.z >= current_z && 
                   point.z <= current_z + params_.height_range) {
                    neutron_cloud->push_back(point);
                   }
            }
            if(static_cast<int>(neutron_cloud->size()) > params_.min_points_in_slice) {
                //detected_z = current_z + params_.height_range;
                detected_z = current_z + params_.height_offset;
                RCLCPP_INFO(rclcpp::get_logger("neutron_detector"), 
                           "Found top region at z=%.3f with %ld points", 
                           detected_z, neutron_cloud->size());
                break;
            }
        }

        return neutron_cloud;
    }


    std::vector<cv::Point2f> NeutronDetector::projectTo2D(const PointCloud& cloud) {
        std::vector<cv::Point2f> points_2d;
        points_2d.reserve(cloud->size());

        for(const auto& point : *cloud) {
            points_2d.emplace_back(point.x, point.y);
        }
        return points_2d;
    }

    
    bool NeutronDetector::fitRectangleAndGetCenter(const std::vector<cv::Point2f>& points,
                                                cv::Point2f& center) {
        if(points.size() < 4) {
            return false;
        }

        //使用opencv拟合最小外接矩形
        cv::RotatedRect rect = cv::minAreaRect(points);
        

        //验证矩形面积
        float area = rect.size.width * rect.size.height;
        if(area < params_.rect_min_area || area > params_.rect_max_area) {
            RCLCPP_DEBUG(rclcpp::get_logger("neutron_detector"), 
                        "Rectangle area out of range: %.4f", area);
            return false;
        }

         
        //返回矩阵中心坐标
        center.x = rect.center.x;
        center.y = rect.center.y;

        RCLCPP_INFO(rclcpp::get_logger("neutron_detector"), 
                   "Fitted rectangle: center(%.3f, %.3f), size(%.3f, %.3f), area=%.4f",
                   rect.center.x, rect.center.y, rect.size.width, rect.size.height, area);
        

        return true;
    }

    /* std::vector<cv::Point2f> NeutronDetector::detectInnerCircleRegion(
        const std::vector<cv::Point2f>& all_points,
        const cv::RotatedRect& outer_rect) {
            std::vector<cv::Point2f> inner_points;

            //计算矩形边界
            cv::Point2f center(outer_rect.center.x, outer_rect.center.y);
            float width = outer_rect.size.width;
            float height = outer_rect.size.height;

            //定义内部区域（假设是矩形中心附近的一个较小区域）
            float inner_width = width * 0.6f;
            float inner_height = height * 0.6f;

            float left = center.x - inner_width / 2;
            float right = center.x + inner_width / 2;
            float top = center.y - inner_height / 2;
            float bottom = center.y + inner_height / 2;

            //提取内部点
            for(const auto& point : all_points) {
                if(point.x >= left && point.x <= right &&
                   point.y >= top && point.y <= bottom) {
                    inner_points.push_back(point);
                   }
            }

            RCLCPP_INFO(rclcpp::get_logger("neutron_detector"), 
                        "Extracted %ld inner points for circle fitting", inner_points.size());
            
            return inner_points;
        }



        bool NeutronDetector::fitCircle(const std::vector<cv::Point2f>& points,
                                        cv::Point2f& center, float& radius) {
            if(points.size() < static_cast<size_t>(params_.circle_min_points)) {
                return false;
            }
            
            //计算点云中心作为初始估计
            cv::Point2f centroid(0, 0);
            for(const auto& point : points){
                centroid.x += point.x;
                centroid.y += point.y;
            }

            centroid.x /= points.size();
            centroid.y /= points.size();

            //计算平均半径
            float sum_radius = 0;
            for(const auto& point : points) {
                float dx = point.x - centroid.x;
                float dy = point.y - centroid.y;
                sum_radius += std::sqrt(dx * dx + dy * dy);
            }
            radius = sum_radius / points.size();

            //验证半径范围
            if(radius < params_.circle_min_radius || radius > params_.circle_max_radius) {
                RCLCPP_DEBUG(rclcpp::get_logger("neutron_detector"), "circle radius out of range: %.4f, radius");
                return false;
            }

            center = centroid;

            RCLCPP_INFO(rclcpp::get_logger("neutron_detector"), 
                        "Fitted circle: center: %.3f, %.3f, radius: %.4f", center.x, center.y, radius);

            return true;
        }


        void NeutronDetector::computeFinalCenter(const cv::Point2f& rect_center,
                                                const cv::Point2f& circle_center,
                                                cv::Point2f& final_center) {
            
            //计算两个中心点的距离
            float dx = rect_center.x - circle_center.x;
            float dy = rect_center.y - circle_center.y;
            float distance = std::sqrt(dx * dx + dy * dy);

            if(distance < params_.center_merge_threshold) {

                //若两个中心点很接近，采用圆形中心（更精确）
                final_center = circle_center;
                RCLCPP_INFO(rclcpp::get_logger("neutron_detector"), "using circle center");
            }
            else{

                //若两个中心点较远，采用矩形中心（更稳定）
                final_center = rect_center;
                RCLCPP_INFO(rclcpp::get_logger("neutron_detector"), "using rectangle center");

            }
        } */

}